#include "bit_array.h"
#include <string.h>
CBit_arr::CBit_arr()
{
	m_data_buf = NULL;
}

CBit_arr::~CBit_arr()
{
}

int  CBit_arr::init(int bytes)
{
	m_data_buf = new unsigned char[bytes];
	m_bit_num = bytes * 8;
	memset(m_data_buf,0,bytes);
	m_bit_idx = 0;
	m_pull_idx = 0;
	return 0;
}

void CBit_arr::push_byte(uint8_t value)
{
	if (m_bit_idx + 8 > m_bit_num)
	{
		return;
	}
	memcpy(m_data_buf + m_bit_idx/8, &value,1);
	m_bit_idx += 8;
}

void CBit_arr::push_uint16(uint16_t value)
{
	if (m_bit_idx + 16 > m_bit_num)
	{
		return;
	}
	memcpy(m_data_buf + m_bit_idx / 8, &value, 2);
	m_bit_idx += 16;
}

int CBit_arr::push_bit(int value, int bit_num)
{
	if (m_bit_idx + bit_num > m_bit_num)
	{
		return 0;
	}
	int ret = 0;
	int byte_idx = size_round_align(m_bit_idx,8);
	int leaf_bit = m_bit_idx % 8;

	uint8_t src_value = m_data_buf[byte_idx];
	uint8_t bit_mask = 0;
	for (auto idx = 0; idx < leaf_bit;idx++)
	{
		bit_mask |= 1 << idx;
	}
	uint8_t dest = (src_value & bit_mask) | ((value << leaf_bit) & 0xFF);
	m_data_buf[byte_idx] = dest;
	if (leaf_bit + bit_num > 8)
	{
		int byte_num = size_round_up(leaf_bit + bit_num, 8);
		for (int i = 0 ; i< byte_num; i++)
		{
			m_data_buf[byte_idx + 1 + i] = (value >> 8*(i+1));
		}
	}

	m_bit_idx += bit_num;

	return m_bit_idx;
}

int CBit_arr::push_bit_byidx(int value, int start_bit, int bit_num)
{
	if (start_bit + bit_num > m_bit_num)
	{
		return -1;
	}

	int ret = 0;
	int byte_idx = size_round_align(start_bit, 8);
	int leaf_bit = start_bit % 8;

	uint8_t src_value = m_data_buf[byte_idx];
	uint8_t bit_mask = 0;
	for (auto idx = 0; idx < leaf_bit; idx++)
	{
		bit_mask |= 1 << idx;
	}
	uint8_t dest = (src_value & bit_mask) | ((value << leaf_bit) & 0xFF);

	m_data_buf[byte_idx] = dest;
	if (leaf_bit + bit_num > 8)
	{
		int byte_num = size_round_up(leaf_bit + bit_num, 8);
		for (int i = 0; i < byte_num; i++)
		{
			m_data_buf[byte_idx + 1 + i] = value >> (8 * (i + 1));
		}
	}

	start_bit += bit_num;

	return start_bit;
}

int CBit_arr::pull_bit(int bit_num)
{
	if (m_pull_idx + bit_num > m_bit_num)
	{
		return -1;
	}

	int ret = 0;
	int byte_idx = size_round_align(m_pull_idx, 8);
	int leaf_bit = m_pull_idx % 8;
	uint8_t src_value  = m_data_buf[byte_idx];
	uint32_t dest_value = 0;
	uint8_t bit_mask   = 0;
	for (auto idx = 0; idx < bit_num; idx++)
	{
		bit_mask |= 1 << idx;
	}

	dest_value = (src_value >> leaf_bit) & bit_mask;
	if (bit_num > 8)
	{
		int byte_num = size_round_up(leaf_bit + bit_num, 8);
		for (int i = 0; i < byte_num; i++)
		{
			dest_value |= m_data_buf[byte_idx + i+1] << (8 * (i + 1));
		}
	}
	m_pull_idx += bit_num;

	return dest_value;
}